clc; clear; close all;

%% 目标点
target = [100, 50]; % 目标点 (x_T, y_T)

%% 迭代求解初始速度
v0_initial = [10, 10]; % 初始猜测速度 (vx0, vy0)
v0_solution = update_initial_velocity(v0_initial, target);

fprintf('命中目标所需初始速度: vx0 = %.2f, vy0 = %.2f\n', v0_solution(1), v0_solution(2));

%% 函数部分

% 计算误差
function error = compute_error(final_state, target)
    xf = final_state(1);
    yf = final_state(2);
    xt = target(1);
    yt = target(2);
    error = [xf - xt, yf - yt]; % 计算误差
end

% 运动学方程 (待补充 f_x, f_y)
function dydt = dynamics(state)
    x = state(1);
    y = state(2);
    vx = state(3);
    vy = state(4);
    
    fx = 0;  % TODO: 填写 x 方向的加速度
    fy = -9.81; % 仅考虑重力

    dydt = [vx; vy; fx; fy];
end

% Runge-Kutta 4 方法
function state_new = runge_kutta4(state, dt)
    k1 = dynamics(state);
    k2 = dynamics(state + 0.5 * dt * k1);
    k3 = dynamics(state + 0.5 * dt * k2);
    k4 = dynamics(state + dt * k3);
    state_new = state + (dt / 6) * (k1 + 2*k2 + 2*k3 + k4);
end

% 轨迹模拟
function final_state = simulate(v0)
    dt = 0.01;
    t_max = 10;
    state = [0, 0, v0(1), v0(2)]; % 初始状态 [x, y, vx, vy]
    t = 0;
    
    while t < t_max
        state = runge_kutta4(state, dt);
        if state(2) < 0 % 命中地面
            break;
        end
        t = t + dt;
    end
    final_state = state;
end

% 使用有限差分计算梯度
function gradient = finite_difference_gradient(v0, target, delta)
    if nargin < 3
        delta = 1e-3; % 设定微小扰动量
    end
    errors = zeros(1,2);
    
    for i = 1:2  % 只对 vx0, vy0 求偏导数
        v0_perturbed = v0;
        v0_perturbed(i) = v0_perturbed(i) + delta;
        
        final_state_perturbed = simulate(v0_perturbed);
        error_perturbed = compute_error(final_state_perturbed, target);
        
        final_state = simulate(v0);
        error = compute_error(final_state, target);
        
        gradient(i) = (error_perturbed(i) - error(i)) / delta;
    end
end

% 梯度下降法迭代求解初始速度
function v0 = update_initial_velocity(v0, target, alpha, tol, max_iter)
    if nargin < 3
        alpha = 0.1; % 学习率
    end
    if nargin < 4
        tol = 1e-3; % 误差阈值
    end
    if nargin < 5
        max_iter = 100; % 最大迭代次数
    end
    
    for iter = 1:max_iter
        final_state = simulate(v0);
        error = compute_error(final_state, target);
        
        if norm(error) < tol  % 误差足够小，停止迭代
            break;
        end
        
        gradient = finite_difference_gradient(v0, target);
        v0 = v0 - alpha * gradient; % 迭代更新速度
    end
end
